#include "SpiderManager.h"
#include "BoneSet.h"

SpiderManager* SpiderManager::_instance = NULL;

	//
	Ogre::String const SpiderManager::B_ROOT = "root";
	//left bones
	const SpiderBones SpiderManager::B_LEFT_1("leg_left_1_1", "leg_left_1_2", "leg_left_1_3" );
	const SpiderBones SpiderManager::B_LEFT_2( "leg_left_2_1", "leg_left_2_2", "leg_left_2_3" );
	const SpiderBones SpiderManager::B_LEFT_3( "leg_left_3_1", "leg_left_3_2", "leg_left_3_3" );
	const SpiderBones SpiderManager::B_LEFT_4( "leg_left_4_1", "leg_left_4_2", "leg_left_4_3" );
	
	//right bones
	const SpiderBones SpiderManager::B_RIGHT_1( "leg_right_1_1", "leg_right_1_2", "leg_right_1_3" );
	const SpiderBones SpiderManager::B_RIGHT_2( "leg_right_2_1", "leg_right_2_2", "leg_right_2_3" );
	const SpiderBones SpiderManager::B_RIGHT_3( "leg_right_3_1", "leg_right_3_2", "leg_right_3_3" );
	const SpiderBones SpiderManager::B_RIGHT_4( "leg_right_4_1", "leg_right_4_2", "leg_right_4_3" );

	//others
	const SpiderBones SpiderManager::B_HEAD( "tail_1", "tail_2", "" );
	const SpiderBones SpiderManager::B_TAIL( "head_1", "head_2", "" );

SpiderManager* SpiderManager::getSingletonPtr()
{
	//static SpiderManager::ReleaseGuard _guard;

	if(_instance == NULL)
		_instance = new SpiderManager();
	
	return _instance;
}

SpiderManager::SpiderManager()
{
	_spiderEntity = NULL;
	_spiderNode = NULL;
	_spiderSkeleton = NULL;
	_sceneManager = NULL;
	_root = NULL;
	
}
void SpiderManager::initialize(Ogre::SceneManager* sceneManager, Ogre::Root* root)
{
	_sceneManager = sceneManager;
	_root = root;
}

void SpiderManager::setSpiderEntity(Ogre::Entity *entity)
{
	
}

void SpiderManager::setSpiderSkeleton(Ogre::Skeleton *skeleton)
{

}

void SpiderManager::setSpiderNode(Ogre::SceneNode *node)
{
	_spiderNode = node;
}

Ogre::Entity* SpiderManager::getSpiderEntity()
{
	return _spiderEntity;
}

Ogre::SceneNode* SpiderManager::getSpiderSceneNode()
{
	return _spiderNode;
}

Ogre::Skeleton* SpiderManager::getSpiderSkeleton()
{
	return _spiderSkeleton;
}

Ogre::SceneManager*	SpiderManager::getSMngr()
{
	return _sceneManager;
}

Ogre::Root*	SpiderManager::getRoot()
{
	return _root;

}
Ogre::Vector3 SpiderManager::wp(Ogre::SceneNode* _node, Ogre::Bone* _bone)// returns given bone's world position
{
	return _node->_getDerivedPosition() + _node->_getDerivedOrientation() * _node->_getDerivedScale() * _bone->_getDerivedPosition();
}
bool SpiderManager::solveIK(Ogre::Vector3 goal, Ogre::Bone* b1, Ogre::Bone* b2, Ogre::Bone* b3)
{
	Ogre::Quaternion q = b2->getOrientation();
	Ogre::Matrix3 m;
	q.ToRotationMatrix(m);
	Ogre::Radian x,y,z;
	m.ToEulerAnglesXYZ(x,y,z);

	// which way bone will turn + or -
	int prefSide = y.valueDegrees() < 0 ? -1 : 1;

	// current vector b3 to b1
	Ogre::Vector3 cv = wp(_spiderNode,b3) - wp(_spiderNode,b1);
	cv.normalise();

	// target vector goal to b1
	Ogre::Vector3 tv = goal - wp(_spiderNode,b1);
	tv.normalise();

	Ogre::Vector3 zax = cv.crossProduct(tv);
	zax.normalise();

	Ogre::Real l = cv.dotProduct(tv);
	Ogre::Radian a = Ogre::Math::ACos(l);

	q.FromAngleAxis(a,zax);
	b1->rotate(q,Ogre::Node::TS_WORLD);

	// UPDATE
	b1->_update(true,false);

	float ex = 0;
	float ey = wp(_spiderNode,b1).distance(goal);

	float l1 = wp(_spiderNode,b1).distance(wp(_spiderNode,b2));
	float l2 = wp(_spiderNode,b2).distance(wp(_spiderNode,b3));

	float cos2 = ((ex * ex) + (ey * ey) - (l1 * l1) - (l2 * l2)) / (2 * l1 * l2);

	if (cos2 >= -1 && cos2 <= 1)
	{
		Ogre::Radian angle = Ogre::Math::ACos(cos2);
		b2->setOrientation(Ogre::Quaternion(angle*prefSide,Ogre::Vector3::UNIT_Y));
		// UPDATE
		b1->_update(true,false);

	}

	// current vector b3 to b1
	cv = wp(_spiderNode,b3) - wp(_spiderNode,b1);
	cv.normalise();

	// target vector goal to b1
	tv = goal - wp(_spiderNode,b1);
	tv.normalise();

	zax = cv.crossProduct(tv);
	zax.normalise();

	l = cv.dotProduct(tv);
	a = Ogre::Math::ACos(l);

	q.FromAngleAxis(a,zax);
	b1->rotate(q,Ogre::Node::TS_WORLD);
	// UPDATE
	b1->_update(true,false);

	return 1;
}
void SpiderManager::updateBones(float deltaTime)
{
	//solveIK(goal,bone1,bone2,bone3);
	
	for(int i = 0; i < boneSets.size(); i++)
	{
		boneSets[i]->update(deltaTime);
	}
}

spiderBoneList&	SpiderManager::getSpiderBonesList()
{
	return _boneList;
}
void SpiderManager::createBoneSets()
{
	//Ogre::Vector3 bone2WP = _spiderMngr->wp(_spiderMngr->getSpiderSceneNode(), _spiderMngr->bone2);
	
	BoneSet* newBoneSet = new BoneSet( B_LEFT_1, wp(_spiderNode, _boneList[B_LEFT_1.BONE2]), LEFT_LEG_1_TIME_OFFSET	);
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_LEFT_2, wp(_spiderNode, _boneList[B_LEFT_2.BONE2]), LEFT_LEG_2_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_LEFT_3, wp(_spiderNode, _boneList[B_LEFT_3.BONE2]), LEFT_LEG_3_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_LEFT_4, wp(_spiderNode, _boneList[B_LEFT_4.BONE2]), LEFT_LEG_4_TIME_OFFSET );
	boneSets.push_back(newBoneSet);

	newBoneSet			= new BoneSet( B_RIGHT_1, wp(_spiderNode, _boneList[B_RIGHT_1.BONE2]), RIGHT_LEG_1_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_RIGHT_2, wp(_spiderNode, _boneList[B_RIGHT_2.BONE2]), RIGHT_LEG_2_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_RIGHT_3, wp(_spiderNode, _boneList[B_RIGHT_3.BONE2]), RIGHT_LEG_3_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
	newBoneSet			= new BoneSet( B_RIGHT_4, wp(_spiderNode, _boneList[B_RIGHT_4.BONE2]), RIGHT_LEG_4_TIME_OFFSET );
	boneSets.push_back(newBoneSet);
}